//
// Created by xujingyi on 2021/5/5.
//

#ifndef ALLIANCE_SENTRY_V2_0_ARMOR_CLASS_H
#define ALLIANCE_SENTRY_V2_0_ARMOR_CLASS_H
#include <opencv2/core.hpp>
#include <vector>
#include <map>
#include <Eigen/Dense>
#include "Setter.h"
#include "additions.h"
namespace rm{
    extern std::map<int, std::string> id2name;  //装甲板id到名称的map
    extern std::map<std::string, int> name2id;  //装甲板名称到id的map
    extern std::map<std::string, int> prior_blue;
    extern std::map<std::string, int> prior_red;

    typedef enum {
        NONE=-1,RED=0,BLUE=1,PURPLE=2     //自瞄状态枚举定义
    }ArmorColor;

    typedef enum {
        NOT_BOX,SMALL_BOX,BIG_BOX     //自瞄状态枚举定义
    }ArmorSize;

    class Marker {
    public:
        Marker(cv::RotatedRect &r,float &peri,ArmorColor color);
        Marker() = default;
        ~Marker()=default;
        cv::Point2f size() const {return _size;}
        float angle() const {return _angle;}
        ArmorColor color() const {return _marker_color;};
        cv::Point2f center() const{return _center;};
        float getHWRatio() const{return _height_width_ratio;};
        float getPeri() const{return _peri;};
        cv::RotatedRect getRect() const{return _mark_rect;};
        std::vector<cv::Point2f> vertexes() const{ return _vertexes;};
        void setOffset(cv::Point2f off);
    private:
        float _angle;
        float _peri;
        float _height_width_ratio;
        cv::Point2f _size,_center;//size(chang,kuan)
        ArmorColor _marker_color=NONE;
        cv::RotatedRect _mark_rect;  //灯条框
        std::vector<cv::Point2f> _vertexes;//左下、左上、右上、右下
    };

    typedef enum{
        UNKNOWN=-1,GROUND=0,HIGHLAND=1 //装甲板所处位置
    }BoxState;

    class MarkerCouple {
    public:
        Marker left_mark;
        Marker right_mark;

        MarkerCouple()=default;
        MarkerCouple(const Marker &left,const Marker &right,const rm::ArmorSize &armorsize);
        ~MarkerCouple()=default;
        ArmorColor color() const {return _couple_color;};
        cv::Point2f center() const {return _couple_center;};
        float getBoxAngle() const {return _box_angle;};
        ArmorSize boxsize() const { return _box_size;};
        cv::Rect2f getNumBox() const { return _num_box;};
        cv::RotatedRect getArmorBox() const { return _armor_box;};
        std::vector<cv::Point2f> vertexes() const{ return _vertexes;};
        cv::Vec3f getEular(){return _rodeTOEular;};
        void setPNPCenter(cv::Point3f _pos,cv::Vec3f eular) ;
        cv::Point3f getPNPCenter() const {return _box_pnp_pos;}

        BoxState getState() const {return _box_state;};
        bool getBlindFlag() const {return _blind_flag;};
        float getAbsYaw() const {return _abs_box_yaw;};

        ArmorSize setSize(const ArmorSize size) {_box_size=size;};
        void setState(const cv::Point3f world_coord,const float abs_pitch,const int mode);
        void setState(BoxState state){_box_state=state;};

        void setBoxState(rm::BoxState st) {_box_state=st;};
        void setBoxsize(rm::ArmorSize sz) {_box_size=sz;_findRoiBoxes();};
        void setOffset(cv::Point2f off);
    private:
        ArmorColor _couple_color=NONE;
        cv::Point2f _couple_center;
        ArmorSize _box_size;
        std::vector<cv::Point2f> _vertexes;
        cv::Point3f _box_pnp_pos;
        cv::Vec3f _rodeTOEular;
        cv::Rect2f _num_box;
        cv::Rect2f _next_track_box;
        cv::RotatedRect _armor_box;
        float _box_angle=0;//point angle
        float _abs_box_yaw=0;
        bool _findRoiBoxes();
        bool _blind_flag=false;
        BoxState _box_state=UNKNOWN;
//        cv::Rect _setNextTrackBox();

    };

    struct BoxPos{
    public:

        Eigen::Matrix<float, 3, 3> Rvec;
        Eigen::Vector3d Tvec;
        BoxPos()=default;
        ~BoxPos()=default;
        cv::Point2f getPicCen2d() const {return _pic_center2f;};
        cv::Point3f getPNPCen3d() const {return _pnp_center3f;};
        cv::Point3f getPixelCen3d() const {return _pixel_center3f;};
        cv::Point3f getWorldCoord() const {return _world_coord;};
        cv::Vec3f getrodetoEularVec() const {return _rodeTOEular;};
//        const cv::Mat_<TYPE_POS> getPNPTvecs() const {return _pnp_tvecs;};
        BoxState getState() const {return _state;};
        void setState(const BoxState state) {_state=state;};

        cv::Point2f setPicCen2d(const cv::Point2f center2d) {_pic_center2f=center2d;};
        void setPNPCenter(const cv::Point2f center2d,const cv::Point3f center3d,const cv::Vec3f rodeToEular){ _pic_center2f=center2d;_pnp_center3f=center3d;_rodeTOEular=rodeToEular;};
        void setPixelCenter(const cv::Point2f center2d,const cv::Point3f center3d){ _pic_center2f=center2d,_pixel_center3f=center3d;};
        void setWorldCoord(const cv::Point3f center3d){_world_coord=center3d;};
        //        void setPredictCenter2d(const cv::Point2f center2d){_predict_center2f=center2d;};
        void setPosMessage(const cv::Point3f center3d){_pos_message=center3d;}
        cv::Point3f getPosMessage(){return _pos_message;}

    private:
        cv::Point2f  _pic_center2f=cv::Point2f();//actually pic_center & compensated pic_center
        cv::Point3f _pnp_center3f=cv::Point3f(),_pixel_center3f=cv::Point3f(),_world_coord=cv::Point3f();
        cv::Point3f _pos_message=cv::Point3f();
        cv::Point3f _transformCamera2PTZ(cv::Point3f &center3d);
        cv::Vec3f _rodeTOEular;
//        cv::Mat_<TYPE_POS> _pnp_tvecs;
        BoxState _state=UNKNOWN;
        float _tmpheight=0;
    };

    class Armor {
    public:
        MarkerCouple couple;
        BoxPos box_pos;//装甲板位姿
        float traject_dwy;//由于重力作用引起的像素y变化
        float move_coeff_angle;//由于底盘运动带来的变化角度
        float delay_t=0;//子弹滞空时间

        Armor()=default;
        Armor(const MarkerCouple &cp,int id,int real): couple(cp),_id(id),_reliability(real){
            box_pos.setState(cp.getState());
            if(cp.getBlindFlag()) box_pos.setPNPCenter(couple.center(),cv::Point3f(couple.getPNPCenter()),couple.getEular());
            else box_pos.setPNPCenter(couple.center(),couple.getPNPCenter(),couple.getEular());
        };
        ~Armor()=default;

        bool operator<(const rm::Armor &box) const;

        int getID() const {return _id;}
        int getIDReal() const {return _reliability;}
        ArmorSize armorSize() const {return couple.boxsize();}
        cv::Point2f center() const {return couple.center();}
        std::vector<cv::Point2f> vertexes() const{ return couple.vertexes();};
        rm::ArmorColor color() const{return couple.color();}


    private:
        int _id=-1;//0-14
        int _reliability=0;
    };
    typedef std::vector<Marker> Markers;
    typedef std::vector<MarkerCouple> MarkerCouples;
    typedef std::vector<Armor> Armors;

    struct GyroInfo{
        GyroInfo(const cv:: Point3f coord,const systime time):_coord(coord),_time(time){};
        cv:: Point3f _coord;
        systime _time;
    };

    class GyroTool{
    public:
        typedef enum {
            LEVO, DEXTRO, UNKNOWN     //自瞄状态枚举定义
        } GyroState;

        GyroTool()=default;
        GyroTool(int id) : _id(id){};
        ~GyroTool() = default;

        RoundQueue<float,50> qEulars;
        RoundQueue<float,50> qOmegas;
        RoundQueue<cv::Point3f,5> qWorldCoords;
//        GyroInfo left_info,middle_info,right_info;
        rm::Armor left_armor=rm::Armor(),right_armor=rm::Armor();
        cv::Point3f gyro_center = cv::Point3f (0, 0, 0);
        float omega=0;

        int getID(){return _id;}
        GyroState getState(){ return _state;}
        void setState(int state){_state=(GyroState)state;}
        void setPosMessage(const cv::Point3f center3d){_gyro_message=center3d;}
        cv::Point3f getPosMessage(){return _gyro_message;}
        void enter(int id){
            _id=id;
        }
        void exit(){
            left_armor=rm::Armor();
            right_armor=rm::Armor();
            _id=-1;
            qEulars.clear();
            qOmegas.clear();
            qWorldCoords.clear();
            gyro_center=cv::Point3f();
            _gyro_message=cv::Point3f();
            _state=UNKNOWN;
            omega=0;
        }
    private:
        GyroState _state=UNKNOWN;
        int _id=-1;
        cv::Point3f _gyro_message=cv::Point3f();
    };
}

#endif //ALLIANCE_SENTRY_V2_0_ARMOR_CLASS_H
